package com.example.originbot;

import android.app.ProgressDialog;
import android.content.Context;
import android.content.pm.PackageManager;
import android.content.res.Configuration;
import android.graphics.Color;
import android.os.AsyncTask;
import android.os.Build;
import android.os.Bundle;
import android.os.Environment;
import android.os.Handler;
import android.os.Process;
import android.text.Editable;
import android.text.InputType;
import android.text.TextWatcher;
import android.util.Log;
import android.view.SurfaceHolder;
import android.view.SurfaceView;
import android.view.View;
import android.view.ViewGroup;
import android.view.Window;
import android.view.WindowManager;
import android.view.animation.Animation;
import android.view.animation.AnimationUtils;
import android.widget.Button;
import android.widget.EditText;
import android.widget.LinearLayout;
import android.widget.TextView;
import android.widget.Toast;

import androidx.annotation.NonNull;
import androidx.appcompat.app.AlertDialog;
import androidx.appcompat.app.AppCompatActivity;
import androidx.core.app.ActivityCompat;

import com.google.protobuf.InvalidProtocolBufferException;
import com.kongqw.rockerlibrary.view.RockerView;

import java.io.File;
import java.io.IOException;
import java.math.BigDecimal;
import java.util.ArrayList;
import java.util.List;
import java.util.Locale;

import ros2.proto.LaserScanOuterClass;
import ros2.proto.OdometryOuterClass;

public class MainActivity extends AppCompatActivity {
    private String[] permiss = {"android.permission.WRITE_EXTERNAL_STORAGE", "android.permission.READ_EXTERNAL_STORAGE"};
    private H264DeCodePlay h264DeCodePlay;
    //    private H265DeCodePlay h265DeCodePlay;
    private String videoPath;
    private H265DeCodePlay h265DeCodePlay;
    private SurfaceView surface;
    private RockerView left, right;
    private TextView linear, angular, frame, battery, text_warn, textAvoid;
    private double linearVal, angularVal;
    private double linearx = 0, angularz = 0;
    private int batteryPrecent = 0;
    private float batteryVoltage = 0;
    private float avoidanceDistance = -1; // 最近障碍物距离
    private int frameCount = 0, laserCount = 0;
    private TCPClient client;
    private int authResult = -1;
    private boolean controlPermission[] = new boolean[2];
    private String notifyMessage = "";
    private ROSProtobuf rosProtobuf;

    @Override
    protected void onCreate(Bundle savedInstanceState) {

        if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.P) {
            WindowManager.LayoutParams lp = getWindow().getAttributes();
            lp.layoutInDisplayCutoutMode = WindowManager.LayoutParams.LAYOUT_IN_DISPLAY_CUTOUT_MODE_SHORT_EDGES;
            getWindow().setAttributes(lp);
        } else {
            getWindow().requestFeature(Window.FEATURE_NO_TITLE);
            getWindow().setFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN, WindowManager.LayoutParams.FLAG_FULLSCREEN);
        }
        final View decorView = getWindow().getDecorView();
        decorView.setSystemUiVisibility(View.SYSTEM_UI_FLAG_LAYOUT_FULLSCREEN | View.SYSTEM_UI_FLAG_LAYOUT_STABLE | View.SYSTEM_UI_FLAG_HIDE_NAVIGATION);

        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);
        checkPermiss();
        initView();
        initRockerView();
        initClient();
        showLoginDialog();
    }

    private void checkPermiss() {
        int code = ActivityCompat.checkSelfPermission(this, permiss[0]);
        if (code != PackageManager.PERMISSION_GRANTED) {
            // 没有写的权限，去申请写的权限
            ActivityCompat.requestPermissions(this, permiss, 11);
        }
    }

    private void setControlPermission(boolean front, boolean back)
    {
        controlPermission[0] = front;
        controlPermission[1] = back;
        if(!controlPermission[0] && !controlPermission[1])
        {
            // 当前速度不是0 停止运动
            if(linearx != 0 || angularz != 0)
            {
                linearVal = 0;
                angularVal = 0;
                rosProtobuf.sendTwist(linearVal, angularVal);
            }
        }
    }

    boolean singleControl = true;

    private void initRockerView() {
        RockerView.OnAngleChangeListener angleChangeListener = new RockerView.OnAngleChangeListener() {
            boolean checkTwistChange(double linearVal, double angularVal) {
                linearVal = BigDecimal.valueOf(linearVal)
                        .setScale(2, BigDecimal.ROUND_HALF_DOWN)
                        .floatValue();
                angularVal = BigDecimal.valueOf(angularVal)
                        .setScale(2, BigDecimal.ROUND_HALF_DOWN)
                        .floatValue();
                if (MainActivity.this.linearVal != linearVal || MainActivity.this.angularVal != angularVal) {
                    MainActivity.this.linearVal = linearVal;
                    MainActivity.this.angularVal = angularVal;
                    return true;
                }
                return false;
            }

            @Override
            public void onStart(RockerView v) {
                if (singleControl) {
                    linearVal = 0;
                    angularVal = 0;
                } else {
                    if (v == left) {
                        linearVal = 0;
                    } else {
                        angularVal = 0;
                    }
                }
                rosProtobuf.sendTwist(linearVal, angularVal);
            }

            private double getLinear(double angle, float percent) {
                float max_speed = 0.1f;
                double linearVal = percent * max_speed;
                if (0 <= angle && angle <= 180) {
                    // 后退
                    linearVal = -linearVal;
                }
                return linearVal;
            }

            private double getAngular(double angle, float percent) {
                // 270 - 360 0 - 90 右转
                // 90 - 180 180 -270 左转
                double angularVal = 0;
                float max_angular = 1f;
                // 右转
                // 0 转速最大 90 转速最小
                if (0 <= angle && angle < 90) {
                    angularVal = max_angular * (90 - angle) / 90;
                    angularVal = -angularVal;
                } // 360 转速最大 270 转速最小
                else if (270 <= angle && angle < 360) {
                    angularVal = max_angular * (angle - 270) / 90;
                    angularVal = -angularVal;
                }
                // 左转
                // 90 -> 180 up
                else if (90 <= angle && angle < 180) {
                    angularVal = max_angular * (angle - 90) / 90;
                }
                // 270 -> 180 up
                else if (180 <= angle && angle < 360) {
                    angularVal = max_angular * (270 - angle) / 90;
                }
                return angularVal;
            }

            @Override
            public void angle(RockerView v, double angle, float percent) {
                if(!controlPermission[0] && !controlPermission[1])
                {
                    onFinish(v);
                    return;
                }
                // 摇杆角度信息
                //    270
                // 180   000
                //    090
                if (singleControl) {
                    double tmpLinear = getLinear(angle, percent);
                    double tmpAngular = getAngular(angle, percent);
                    // 值跟上次一样 不发送 避免重复发送
                    if (!checkTwistChange(tmpLinear, tmpAngular)) {
                        return;
                    }
                } else {
                    if (v == left) {
                        getLinear(angle, percent);
                    } else {
                        getAngular(angle, percent);
                    }
                }
                if(!controlPermission[0] && linearVal > 0)
                {
                    // 禁止前进
                    onFinish(v);
                    return;
                }
                if(!controlPermission[1] && linearVal < 0)
                {
                    // 禁止后退
                    onFinish(v);
                    return;
                }
                // 线速度 +前进 -后退
                // 角速度 +左转（逆时针） -右转（顺时针）
                // System.out.println(String.format(Locale.CHINA ,"angle:%.2f percent:%.2f", angle, percent));
                rosProtobuf.sendTwist(linearVal, angularVal);
            }

            @Override
            public void onFinish(RockerView v) {
                if (singleControl) {
                    linearVal = 0;
                    angularVal = 0;
                } else {
                    if (v == left) {
                        linearVal = 0;
                    } else {
                        angularVal = 0;
                    }
                }
                rosProtobuf.sendTwist(linearVal, angularVal);
            }
        };
        left = findViewById(R.id.rockerViewLeft);
        right = findViewById(R.id.rockerViewRight);
        left.setCallBackMode(RockerView.CallBackMode.CALL_BACK_MODE_STATE_CHANGE);
        right.setCallBackMode(RockerView.CallBackMode.CALL_BACK_MODE_STATE_CHANGE);
        left.setOnAngleChangeListener(angleChangeListener);
        right.setOnAngleChangeListener(angleChangeListener);
    }

    private void initView() {
        File dir = getExternalFilesDir(Environment.DIRECTORY_DOWNLOADS);
        if (!dir.exists()) dir.mkdirs();
        // final File file = new File(dir, "output.h264");
        // final File file = new File(dir, "output.h265");
        final File file = new File("/storage/emulated/0/output.h265");
        if (!file.exists()) {
            Log.e("Tag", "文件不存在:" + file.getAbsolutePath());
            // return;
        }
        videoPath = file.getAbsolutePath();
        surface = findViewById(R.id.surface);
        linear = findViewById(R.id.linear);
        angular = findViewById(R.id.angular);
        frame = findViewById(R.id.text_frame_rate);
        battery = findViewById(R.id.text_battery);
        text_warn = findViewById(R.id.text_warn);
        Animation anim = AnimationUtils.loadAnimation(this, R.anim.fade_animation);
        text_warn.startAnimation(anim);
        textAvoid = findViewById(R.id.text_avoid);
        final SurfaceHolder holder = surface.getHolder();
        holder.addCallback(new SurfaceHolder.Callback() {
            @Override
            public void surfaceCreated(@NonNull SurfaceHolder surfaceHolder) {
                if (surfaceHeight == 0)
                    surfaceHeight = surfaceHolder.getSurfaceFrame().height();
                // surface.setRotation(90);
                ViewGroup.LayoutParams layoutParams = surface.getLayoutParams();
//                layoutParams.width = surfaceHolder.getSurfaceFrame().width();
//                layoutParams.height = layoutParams.width * 544 / 960;
                layoutParams.height = surfaceHolder.getSurfaceFrame().height();
                layoutParams.width = layoutParams.height * 960 / 544;
                // surface.setLayoutParams(layoutParams);
                surfaceHolder.setFixedSize(layoutParams.width, layoutParams.height);
//                h264DeCodePlay = new H264DeCodePlay(videoPath, holder.getSurface());
//                h264DeCodePlay.decodePlay();
                h265DeCodePlay = new H265DeCodePlay(videoPath, holder.getSurface());
                // h265DeCodePlay.decodePlay();
                h265DeCodePlay.h26xPlay();
            }

            @Override
            public void surfaceChanged(@NonNull SurfaceHolder surfaceHolder, int i, int i1, int i2) {

            }

            @Override
            public void surfaceDestroyed(@NonNull SurfaceHolder surfaceHolder) {

            }
        });
        updateUi();
        handler.post(runnable);
    }

    private void updateUi() {
        if (linear != null && angular != null && battery != null) {
            linear.setText(String.format(Locale.CHINA, "线速度: %.3f m/s", linearx));
            angular.setText(String.format(Locale.CHINA, "角速度: %.3f °/s", angularz / (2 * Math.PI) * 360));
            battery.setText(String.format(Locale.CHINA, "🔋 %2d%% %2.2fv", batteryPrecent, batteryVoltage));
            if (-1 == avoidanceDistance) {
                textAvoid.setVisibility(View.INVISIBLE);
            } else {
                textAvoid.setVisibility(View.VISIBLE);
            }
            if (avoidanceDistance <= 0.2) {
                textAvoid.setTextColor(Color.RED);
            } else {
                textAvoid.setTextColor(Color.GREEN);
            }
            textAvoid.setText(String.format(Locale.CHINA, "障碍物:%.2fm", avoidanceDistance));
        }
    }

    private Runnable updateUiRunnable = () -> {
        updateUi();
    };

    Handler handler = new Handler();
    Runnable runnable = new Runnable() {
        @Override
        public void run() {
            // 在此处执行定时任务
            // 例如，更新UI或发送请求
            // ...
            if (0 == frameCount || 0 == laserCount) {
                setControlPermission(false, false);
                text_warn.setVisibility(View.VISIBLE);
                text_warn.setText(String.format(Locale.CHINA, "%s%s暂时不能控制", frameCount > 0 ? "" : "无视频信号 ", laserCount > 0 ? "" : "无雷达信号 "));
                if (!client.isAvaliable() && authResult != -1) {
                    Toast.makeText(MainActivity.this, "服务器链接已断开", Toast.LENGTH_SHORT).show();
                    finish();
                }
                if(text_warn.getAnimation() == null)
                {
                    Animation animation = AnimationUtils.loadAnimation(MainActivity.this, R.anim.fade_animation);
                    text_warn.startAnimation(animation);
                }
            } else {
                if (notifyMessage.equals("")) {
                    if(text_warn.getAnimation() != null && text_warn.getAnimation().hasStarted())
                    {
                        Animation animation = AnimationUtils.loadAnimation(MainActivity.this, R.anim.no_anim);
                        text_warn.startAnimation(animation);
                    }
                    text_warn.setVisibility(View.GONE);
                } else {
                    text_warn.setVisibility(View.VISIBLE);
                    text_warn.setText(notifyMessage);
                    if (!controlPermission[0]) {
                        text_warn.append("\n已禁止前进");
                    }
                    if (!controlPermission[1]) {
                        text_warn.append("\n已禁止后退");
                    }
                    if(text_warn.getAnimation() == null)
                    {
                        Animation animation = AnimationUtils.loadAnimation(MainActivity.this, R.anim.fade_animation);
                        text_warn.startAnimation(animation);
                    }
                }
            }
            frame.setText(String.format(Locale.CHINA, "帧率:%dfps", frameCount));
            frameCount = 0;
            laserCount = 0;
            handler.postDelayed(this, 1000);  // 每1000毫秒（1秒）执行一次
        }
    };

    static int surfaceHeight = 0;

    @Override
    protected void onResume() {
        super.onResume();
    }

    @Override
    public void onConfigurationChanged(@NonNull Configuration newConfig) {
        super.onConfigurationChanged(newConfig);
    }

    private void avoidance(LaserScanOuterClass.LaserScan laser) {
        int maxIndex = -1;
        float maxValue = Float.MIN_VALUE;
        int minIndex = -1;
        float minValue = Float.MAX_VALUE;
        laserCount++;

        List<Float> ranges = laser.getRangesList();

        ArrayList<Integer> warnDistance = new ArrayList<>();
        ArrayList<Integer> dangerDistance = new ArrayList<>();

        for (int i = 0; i < ranges.size(); i++) {
            if (laser.getRangeMin() <= ranges.get(i) && ranges.get(i) <= laser.getRangeMax()) {
                if (ranges.get(i) > maxValue) {
                    maxValue = ranges.get(i);
                    maxIndex = i;
                }
                if (ranges.get(i) < minValue) {
                    minValue = ranges.get(i);
                    minIndex = i;
                }
                if (ranges.get(i) < 0.5) {
                    warnDistance.add(i);
                }
                if (ranges.get(i) < 0.2) {
                    dangerDistance.add(i);
                }
            }
        }

        if (minIndex == -1) {
            setControlPermission(true, true);
            notifyMessage = "";
            avoidanceDistance = -1;
            return;
        }

        int dangeNums[] = new int[4]; // 前后左右
        for (int index = 0; index < dangerDistance.size(); index++) {
            int degree = (int) (dangerDistance.get(index) * laser.getAngleIncrement() / Math.PI
                    * 180);
            if (90 <= degree && degree <= 270) {
                dangeNums[0]++; // 前
            }
            if (270 <= degree || degree <= 90) {
                dangeNums[1]++; // 后
            }
            if (180 <= degree && degree <= 360) {
                dangeNums[2]++; // 左
            }
            if (0 <= degree && degree <= 180) {
                dangeNums[3]++; // 右
            }
        }
        int warnNums[] = new int[4]; // 前后左右
        for (int index = 0; index < warnDistance.size(); index++) {
            int degree = (int) (warnDistance.get(index) * laser.getAngleIncrement() / Math.PI
                    * 180);
            if (90 <= degree && degree <= 270) {
                warnNums[0]++; // 前
            }
            if (270 <= degree || degree <= 90) {
                warnNums[1]++; // 后
            }
            if (180 <= degree && degree <= 360) {
                warnNums[2]++; // 左
            }
            if (0 <= degree && degree <= 180) {
                warnNums[3]++; // 右
            }
        }
        System.out.println(String.format("warnNums: %d %d %d %d", warnNums[0], warnNums[1], warnNums[2], warnNums[3]));
        System.out.println(
                String.format("dangeNums: %d %d %d %d", dangeNums[0], dangeNums[1], dangeNums[2], dangeNums[3]));
        //    前180
        // 左270 右090
        //    后000
        avoidanceDistance = minValue;
        boolean notify = false;
        StringBuffer notice = new StringBuffer("请注意 ");
        if (warnNums[0] >= 50) {
            notice.append("前方 ");
            notify = true;
        }
        if (warnNums[1] >= 50) {
            notice.append("后方 ");
            notify = true;
        }
        if (warnNums[2] >= 50) {
            notice.append("左方 ");
            notify = true;
        }
        if (warnNums[3] >= 50) {
            notice.append("右方 ");
            notify = true;
        }
        notice.append("障碍物");
        System.out.println(notice.toString());
        if(notify) {
            notifyMessage = notice.toString();
        } else {
            notifyMessage = "";
        }

        if (dangeNums[0] > 50) {
            System.out.println("禁止前进");
            setControlPermission(false, controlPermission[1]);
        } else {
            setControlPermission(true, controlPermission[1]);
        }
        if (dangeNums[1] > 50) {
            System.out.println("禁止后退");
            setControlPermission( controlPermission[0], false);
        } else {
            setControlPermission( controlPermission[0], true);
        }
    }

    /**
     * 初始化TCP客户端 处理接收到的数据
     * */
    void initClient() {
        client = new TCPClient() {
            @Override
            public void processPacket(byte[] packet) {
                rosProtobuf.parseFromBytes(packet, new ROSProtobuf.ReceivedInterface() {
                    void receivedH26XFrame(H26XFrameOuterClass.H26XFrame h26x) {
                        if (h265DeCodePlay != null) {
                            byte data[] = h26x.getData().toByteArray();
                            h265DeCodePlay.processData(data, data.length);
                            frameCount++;
                        }
                    }
                    void receivedOdometry(OdometryOuterClass.Odometry odom) {
                        linearx = odom.getTwist().getTwist().getLinear().getX();
                        angularz = odom.getTwist().getTwist().getAngular().getZ();
                        runOnUiThread(updateUiRunnable);
                    }

                    void receivedOriginBotStatus(OriginBotStatusOuterClass.OriginBotStatus status) {
                        batteryPrecent = status.getBatteryPrecent();
                        batteryVoltage = status.getBatteryVoltage();
                    }
                    void receivedDeviceAuth(DeviceAuthOuterClass.DeviceAuth auth) {
                        if (auth.hasAuthResult()) {
                            DeviceAuthOuterClass.DeviceAuth.AuthResult res = auth.getAuthResult();
                            if (res == DeviceAuthOuterClass.DeviceAuth.AuthResult.SUCCESS) {

                            }
                            authResult = res.getNumber();
                        }
                    }
                    void receivedLaserScan(LaserScanOuterClass.LaserScan laser) {
                        //     前180
                        //左360      右090
                        //     后000
                        avoidance(laser);
                    }
                });
            }
        };
        rosProtobuf = new ROSProtobuf(client);
    }

    private void startClientThread() {
        Thread t = new Thread(() -> {
            while (true) {
                if (!client.sendData()) {
                    break;
                }
            }
        });
        t.setName("客户端发送线程");
        t.start();
        Thread t2 = new Thread(() -> {
            client.beginReceive();
        });
        t2.setName("客户端接收线程");
        t2.start();
    }

    private void showLoginDialog() {
        final EditText usernameEditText = new EditText(this);
        final EditText passwordEditText = new EditText(this);
        usernameEditText.setText("panxuesen");
        passwordEditText.setText("panxuesen");
        usernameEditText.setHint("请输入用户名");
        passwordEditText.setHint("请输入密码");
        passwordEditText.setInputType(InputType.TYPE_CLASS_TEXT | InputType.TYPE_TEXT_VARIATION_PASSWORD);

        LinearLayout layout = new LinearLayout(this);
        layout.setOrientation(LinearLayout.VERTICAL);
        layout.addView(usernameEditText);
        layout.addView(passwordEditText);

        final AlertDialog dialog = new AlertDialog.Builder(this)
                .setTitle("Login")
                .setView(layout)
                .setPositiveButton("Confirm", null)
                .setCancelable(false)
                .create();

        dialog.setOnShowListener(d -> {
            final Button confirmButton = dialog.getButton(AlertDialog.BUTTON_POSITIVE);
            confirmButton.setEnabled(!usernameEditText.getText().toString().equals("") && !passwordEditText.getText().toString().equals(""));
            confirmButton.setOnClickListener(new View.OnClickListener() {
                @Override
                public void onClick(View view) {
                    String username = usernameEditText.getText().toString();
                    String password = passwordEditText.getText().toString();
                    // do something with the input
                    new AsyncTaskExample(MainActivity.this).execute(username, password);
                    dialog.cancel();
                }
            });

            TextWatcher textWatcher = new TextWatcher() {
                @Override
                public void beforeTextChanged(CharSequence s, int start, int count, int after) {
                }

                @Override
                public void onTextChanged(CharSequence s, int start, int before, int count) {
                }

                @Override
                public void afterTextChanged(Editable s) {
                    String username = usernameEditText.getText().toString();
                    String password = passwordEditText.getText().toString();
                    if (!username.isEmpty() && !password.isEmpty()) {
                        confirmButton.setEnabled(true);
                    } else {
                        confirmButton.setEnabled(false);
                    }
                }
            };

            usernameEditText.addTextChangedListener(textWatcher);
            passwordEditText.addTextChangedListener(textWatcher);
        });
        dialog.show();
    }

    class AsyncTaskExample extends AsyncTask<String, String, Boolean> {
        private ProgressDialog progressDialog;
        private Context context;

        AsyncTaskExample(Context context) {
            this.context = context;
        }

        @Override
        protected void onPreExecute() {
            progressDialog = new ProgressDialog(context);
            progressDialog.setMessage("Please wait...");
            progressDialog.setCancelable(false);
            progressDialog.show();
        }

        @Override
        protected void onProgressUpdate(String... values) {
            super.onProgressUpdate(values);
            progressDialog.setMessage(values[0]);
        }

        private void sleep(long millis) {
            try {
                Thread.sleep(millis);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }

        @Override
        protected Boolean doInBackground(String... values) {
            // Perform a long task here
            publishProgress("正在连接服务器，请稍后...");
            try {
                client.linkServer("192.168.31.1", 12345);
            } catch (IOException e) {
                e.printStackTrace();
                publishProgress("连接服务器失败:" + e.getMessage());
                sleep(3000);
                return false;
            }
            authResult = -1;
            publishProgress("正在验证用户:" + values[0]);
            if (!rosProtobuf.sendAuth(values[0], values[1])) {
                publishProgress("服务器通信失败");
                sleep(3000);
                return false;
            }
            startClientThread();
            publishProgress("正在等待验证结果");
            for (int i = 0; i < 10; i++) {
                if (!client.isAvaliable()) {
                    if (authResult == -1) {
                        publishProgress("服务器通信失败");
                        sleep(3000);
                        return false;
                    }
                }
                if (authResult != -1) {
                    break;
                }
                sleep(1000);
            }
            if (authResult == -1) {
                publishProgress("验证超时！");
                sleep(3000);
                return false;
            }
            if (authResult > 0) {
                publishProgress("用户名或密码错误:" + authResult);
                sleep(3000);
                return false;
            }
            return true;
        }

        @Override
        protected void onPostExecute(Boolean result) {
            progressDialog.dismiss();
            if (result) {
                Toast.makeText(context, "成功", Toast.LENGTH_SHORT).show();
            } else {
                Toast.makeText(context, "失败", Toast.LENGTH_SHORT).show();
                finish();
            }
        }
    }


    @Override
    protected void onPause() {
        super.onPause();
        android.os.Process.killProcess(Process.myPid());
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
        android.os.Process.killProcess(Process.myPid());
    }
}